package r2d2.behavior;

import lejos.robotics.subsumption.Behavior;
import r2d2.robot.Robot;
import r2d2.sensors.CollisionSensor.ObstacleDetection;

public class CollisionDetector implements Behavior {

	private Robot compotents;

	public CollisionDetector(Robot robot) {
		this.compotents = robot;
	}

	@Override
	public void action() {
		compotents.setInCollision(true);
		compotents.getPilot().stop();
		compotents.getPilot().setTurnSpeed(50);
		compotents.getPilot().rotate(360, true);
		ObstacleDetection value = compotents.getSensors().getCollisionSensor()
				.getZone();

		while (!value.equals(ObstacleDetection.NO_DETECTION)) {

			// System.out.println(value.name());
			value = compotents.getSensors().getCollisionSensor().getZone();
			try {
				Thread.sleep(20);
			} catch (InterruptedException e) {
			}
		}
		compotents.getPilot().stop();
		System.out.println("end collision");
		compotents.setInCollision(false);

	}

	@Override
	public void suppress() {
		compotents.setInCollision(false);
		compotents.getPilot().stop();
	}

	@Override
	public boolean takeControl() {
		ObstacleDetection value = compotents.getSensors().getCollisionSensor()
				.getZone();
		if (!value.equals(ObstacleDetection.NO_DETECTION)
				&& !compotents.isInCollision()) {

			System.out.println("collision" + value.name());
			return true;

		}
		return false;
	}

}